/**
 * @file omni_chassis.c
 * @author simon
 * @brief 横辊子全向轮运动学相关
 * @version 0.1
 * @date 2021-04-25
 * 
 * @copyright Copyright (c) 2021
 * 
 */
#include "base_chassis.h"
#include "omni_chassis.h"
#include "chassis_common_config.h"
#include "can_utils.h"
#include "utils.h"
#include "handle.h"
#include "main.h"
#include "stdio.h"
#include "stdlib.h"

uint32_t IR_start_flag = 0; //等待状态标志
uint32_t IR_cnt_ms = 0;     //ac区等待计时，b区防卡死计时
uint32_t c_last_ms = 0;
extern uint8_t IR_status;
#ifdef USE_CHASSIS_OMNI

OmniChassis_t OmniChassis;
//static float InstallAngle[4] = {-2.3561945625, 2.3561945625, -0.7853981875, 0.7853981875}; // 车轮正方形与底盘坐标系x轴的夹角/rad
static float InstallAngle[4] = {-0.7853981875, 0.7853981875, 2.3561945625, -2.3561945625};
//static CAN_Message_u MotorRpmCANMsg[4];
CAN_Message_u MotorRpmCANMsg[4];
char MotorCANSendFlag = 0; // 需要在定时中断中置1，控制发送频率

/**
 * @brief 横辊子全向轮运动学解算
 * 
 * @param target_speed m/s
 * @param target_dir rad
 * @param target_omega rad/s
 */
void Omni_ChassisMove(float target_speed, float target_dir, float target_omega)
{
  //LIMIT(target_speed, DRIVE_WHEEL_MAX_SPEED);

  float absolute_angle_offset = 0; // 使用绝对坐标时，根据全场定位测得的偏航角进行补偿
  if (OmniChassis.base->pos_mode == POS_MODE_ABSOLUTE)
  {
    absolute_angle_offset = OmniChassis.base->PostureStatus.yaw - 1.5708; // 车头初始方向90°
  }
  target_dir += absolute_angle_offset;

  // 将目标速度向量投影到每个驱动轮上
  float speed_out_0 = (target_speed * cos(InstallAngle[0] - target_dir));
  float speed_out_1 = (target_speed * cos(InstallAngle[1] - target_dir));
  float speed_out_2 = (target_speed * cos(InstallAngle[2] - target_dir));
  float speed_out_3 = (target_speed * cos(InstallAngle[3] - target_dir));

  // 与角速度在每个轮上的线速度合成
  float diagonal_length = Wheel2ChassisCenter;
  float motor_speed[4];
  motor_speed[0] = (speed_out_0 - target_omega * diagonal_length);
  motor_speed[1] = (speed_out_1 - target_omega * diagonal_length);
  motor_speed[2] = (speed_out_2 - target_omega * diagonal_length);
  motor_speed[3] = (speed_out_3 - target_omega * diagonal_length);
  int16_t motor_speed_set[4];

  if (OmniChassis.motor_can_flag)
  {
    for (int i = 0; i < 4; i++)
    {
      motor_speed_set[i] = (int16_t)(motor_speed[i] / DRIVE_WHEEL_RADIUS * 30.0f / PI);
    }
    DJI_velCtrAll(motor_speed_set);
    //for (int i = 0; i < 4; i++)
    //{
    //    MotorRpmCANMsg[i].in[0] = 1;
    //    MotorRpmCANMsg[i].in[1] = (int)(motor_speed[i] / DRIVE_WHEEL_RADIUS * 30.0f / PI); //m/s转为rpm
    //    CAN_SendStdMsg(OmniChassis.drive_motors[i].can_id, &MotorRpmCANMsg[i]);
    //    HAL_Delay(1);
    //}
    OmniChassis.motor_can_flag = 0;
  }
}

/**
 * @brief 初始化函数
 * 
 */
void OmniChassis_Init()
{
  BaseChassis_Init();
  OmniChassis.base = &BaseChassis;
  OmniChassis.base->fChassisMove = Omni_ChassisMove;
  for (int i = 0; i < 4; i++)
  {
    // VESC_Init(&OmniChassis.drive_motors[i],i + OMNI_DRIVE_MOTOR_BASE_CANID,(VESCMode)VESC_RPM); // 若使用本杰明驱动器
    MotorDriver_Init(&OmniChassis.drive_motors[i], i + OMNI_DRIVE_MOTOR_BASE_CANID, MTR_CTRL_RPM); // 若使用BUPT自制驱动卡
  }
  OmniChassis.motor_can_flag = 0;
}

/**
 * @brief 横辊子全向轮状态机
 * 
 */
extern int last_point;
extern int inwaitexeflag;
extern uint8_t YawTuning_Finished;
extern int cquxishu;
extern int dquxishu;
extern int penshuicishu[19];
void OmniChassis_Exe()
{
  Chassis_UpdatePostureStatus();
  switch (OmniChassis.base->ctrl_mode)
  {
  case CTRL_MODE_NONE:
    break;
  case CTRL_MODE_WAIT:
    Omni_ChassisMove(0, 0, 0);
    if (IR_start_flag == 0 && IR_status != CIR && IR_status != DIR)
    {
      if (last_point == 6)
      {
        IR_status = BIR;
        IR_cnt_ms = 0;
        OmniChassis.base->ctrl_mode = CTRL_MODE_TRACK;
        break;
      }
      OmniChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
    }
    break;
  case CTRL_MODE_HANDLE:
    OmniChassis.base->pos_mode = POS_MODE_ABSOLUTE;
    if (IR_status == BIR)
      Omni_ChassisMove(0.5, 3.14159275, 0);
    else if(last_point>=11)
      Omni_ChassisMove(0.5, -1.571, 0);
    //Handle_Exe();
    //OmniChassis.base->fChassisMove(OmniChassis.base->target_speed, OmniChassis.base->target_dir, OmniChassis.base->target_omega); // 底层执行函数
    break;
  case CTRL_MODE_CMD:
    OmniChassis.base->pos_mode = POS_MODE_ABSOLUTE;
    OmniChassis.base->target_speed = CMD_TargetSpeed;
    OmniChassis.base->target_dir = (CMD_TargetDir);
    OmniChassis.base->target_omega = CMD_TargetOmega;
    OmniChassis.base->fChassisMove(OmniChassis.base->target_speed, OmniChassis.base->target_dir, OmniChassis.base->target_omega); // 底层执行函数
    break;
  case CTRL_MODE_GO_TO_POINT:
    OmniChassis.base->pos_mode = POS_MODE_ABSOLUTE;
    Chassis_Go2Point(OmniChassis.base->Go2PointStatus.target_point,
                     OmniChassis.base->Go2PointStatus.target_yaw,
                     OmniChassis.base->Go2PointStatus.start_speed,
                     OmniChassis.base->Go2PointStatus.final_speed);
    OmniChassis.base->fChassisMove(OmniChassis.base->target_speed, OmniChassis.base->target_dir, OmniChassis.base->target_omega);
    break;
  case CTRL_MODE_GO_TO_POINT_GROUP:
    OmniChassis.base->pos_mode = POS_MODE_ABSOLUTE;
    for (int i = 1; i < 5; i++)
      Chassis_TrackPoints(i);
    break;
  case CTRL_MODE_TRACK:
    OmniChassis.base->pos_mode = POS_MODE_ABSOLUTE;
    // Chassis_TrackPathSets(OmniChassis.base->TrackStatus.track_path_index);
    //if(HAL_GPIO_ReadPin(kaiguan_GPIO_Port, kaiguan_Pin)==0)
    Chassis_TrackPath(points_pos1, points_pos1_num);
    //else if(HAL_GPIO_ReadPin(kaiguan_GPIO_Port, kaiguan_Pin)==1)
    //Chassis_TrackPath(points_pos2, points_pos2_num);
    OmniChassis.base->fChassisMove(OmniChassis.base->target_speed, OmniChassis.base->target_dir, OmniChassis.base->target_omega);
    if (BaseChassis.TrackStatus.finished)
    {
      OmniChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
      IR_status = CIR;
      for (int i = 0; i < 19; i++)
      {
        penshuicishu[i] = 0;
      }
      OmniChassis.base->Go2PointStatus.enable = 0;
      inwaitexeflag = 0;
      IR_start_flag = 1;
      c_last_ms = 0;
    }
    break;
  case CTRL_MODE_TUNING:
    //Chassis_jimutuning();
    // OmniChassis.base->fChassisMove(0, 0, OmniChassis.base->target_omega);
    //Chassis_YawTuning(CMD_TargetYaw);
    if (last_point == 12)
      Chassis_YawTuning(-1.57);
    else if (last_point == 20)
    {
      Chassis_YawTuning(1.57);
      for (int i = 0; i < 10; i++)
        HAL_UART_Transmit(&huart4, "3", 1, 0xFFFF);
    }
    OmniChassis.base->fChassisMove(OmniChassis.base->target_speed, OmniChassis.base->target_dir, OmniChassis.base->target_omega); // 底层执行函数
    if (YawTuning_Finished)
    {
      for (int i = 0; i < 19; i++)
      {
        penshuicishu[i] = 0;
      }
      YawTuning_Finished = 0;
      OmniChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
      OmniChassis.base->Go2PointStatus.enable = 0;
      inwaitexeflag = 0;
      IR_start_flag = 1;
      c_last_ms = 0;
      if (last_point == 12)
        cquxishu--;
      else if (last_point == 20)
        dquxishu--;
    }
    break;
  default:
    break;
  }
}

/**
 * @brief 灌溉区域状态机
 * inwaitexeflag 车停住后首次进入等待状态
 */
#define SECTION_A_1_X 669.801819f
#define SECTION_A_1_Y 2.422112f
#define SECTION_A_2_X 1253.670776f
#define SECTION_A_2_Y 30.543396f
#define SECTION_A_3_X 1859.146362f
#define SECTION_A_3_Y 40.266155f
#define SECTION_A_4_X 2473.761230f
#define SECTION_A_4_Y 81.455299f
#define SECTION_A_5_X 3021.951172f
#define SECTION_A_5_Y 107.30397f
#define SECTION_A_6_X 3619.616943f
#define SECTION_A_6_Y 139.48716f
#define SECTION_A_7_X 4176.198242f
#define SECTION_A_7_Y 168.15382f

#define SECTION_C_1_X -9.678179f
#define SECTION_C_1_Y 2648.077881f
#define SECTION_C_2_X 964.704407f
#define SECTION_C_2_Y 2632.325928f
#define SECTION_C_3_X 2108.157715f
#define SECTION_C_3_Y 2650.721436f
#define SECTION_C_4_X 3203.867188f
#define SECTION_C_4_Y 2683.698975f
#define SECTION_C_5_X 4125.946777f
#define SECTION_C_5_Y 2672.428955f
#define SECTION_C_6_X 2931.826660f
#define SECTION_C_6_Y 2744.661865f
#define SECTION_C_7_X 1789.487305f
#define SECTION_C_7_Y 2812.951416f
#define SECTION_C_8_X 723.025574f
#define SECTION_C_8_Y 2897.465576f
#define SECTION_C_9_X -223.958725f
#define SECTION_C_9_Y 2963.049561f

#define SECTION_D_1_X -93.796608f
#define SECTION_D_1_Y 4373.880859f
#define SECTION_D_2_X 1351.965576f
#define SECTION_D_2_Y 4309.728516f
#define SECTION_D_3_X 2495.313232f
#define SECTION_D_3_Y 4328.682617f
#define SECTION_D_4_X 3944.525391f
#define SECTION_D_4_Y 4239.363281f
#define SECTION_D_5_X 2780.685303f
#define SECTION_D_5_Y 4347.527344f
#define SECTION_D_6_X 1625.895142f
#define SECTION_D_6_Y 4466.136719f
#define SECTION_D_7_X 162.001160f
#define SECTION_D_7_Y 4687.421387f
int penshuicishu[19] = {0};
int point_groupx[23] = {SECTION_A_1_X, SECTION_A_2_X, SECTION_A_3_X, SECTION_A_4_X, SECTION_A_5_X, SECTION_A_6_X, 4001,
                        SECTION_C_1_X, SECTION_C_2_X, SECTION_C_3_X, SECTION_C_4_X, SECTION_C_5_X, SECTION_C_6_X, SECTION_C_7_X, SECTION_C_8_X, SECTION_C_9_X,
                        SECTION_D_1_X, SECTION_D_2_X, SECTION_D_3_X, SECTION_D_4_X, SECTION_D_5_X, SECTION_D_6_X, SECTION_D_7_X}; //(4595,-180)为理论十字点，（4660，-75）为实际十字点
int point_groupy[23] = {SECTION_A_1_Y, SECTION_A_2_Y, SECTION_A_3_Y, SECTION_A_4_Y, SECTION_A_5_Y, SECTION_A_6_Y, 147,
                        SECTION_C_1_Y, SECTION_C_2_Y, SECTION_C_3_Y, SECTION_C_4_Y, SECTION_C_5_Y, SECTION_C_6_Y, SECTION_C_7_Y, SECTION_C_8_Y, SECTION_C_9_Y,
                        SECTION_D_1_Y, SECTION_D_2_Y, SECTION_D_3_Y, SECTION_D_4_Y, SECTION_D_5_Y, SECTION_D_6_Y, SECTION_D_7_Y};
int last_point = 0;      //其实是从1开始
extern uint32_t test_ms; //喷水时间计时
extern uint8_t openmv[19];
int opencv[20] = {100, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 1, 2, 3, 101};
uint8_t openmv1[20] = {100, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 101};
uint8_t openmv2[20] = {100, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 101};
uint8_t openmv3[20] = {100, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 101};
int cquxishu = 7;
int dquxishu = 0;
int inwaitexeflag = 0; //a区车停住后首次进入等待状态，c区根据激光电平变化，负责控制4.5s计时器只归零一次（类似a区startflag）
int B_flag = 0;        //b区判断标志，每过一个区域+1
int B_start_flag = 1;  // b区y位置判断标志，为了不让在a区时播报
int C_flag = 1;        //c区控制0.5秒计时器
int D_flag = 1;
int last_laser = 1;
float lastposture_x = 0;
float lastposture_y = 0;
int kasi_flag;
int distance = 0;    //D区花盆距离激光雷达的距离
int last_B_flag = 0; //用于B区定时器归零，检测B_flag变化
extern BaseChassis_s BaseChassis;
extern uint8_t D_boardarea;
int detect_num = 0;
int totalD = 0;
char str1[6];
char str2[6];
char str3[6];
char *s1 = str1;
char *s2 = str2;
char *s3 = str3;
extern char HMI_SWITCH_AUDIO_18[16];
extern char HMI_SWITCH_AUDIO_19[16];
extern char HMI_SWITCH_AUDIO_20[16];
extern char HMI_SWITCH_AUDIO_21[16];
int size = 0;
void IR_exe()
{
  if (last_point == 0)
  {
    for (int i = 1; i < 19; i++)
    {
      if (openmv[i] == 1)
        openmv1[i]++;
      else if (openmv[i] == 2)
        openmv2[i]++;
      else if (openmv[i] == 3)
        openmv3[i]++;
    }
  }

  switch (IR_status)
  {
  case AIR:
    if (OmniChassis.base->Go2PointStatus.enable == 0 && last_point != 15)
    {
      inwaitexeflag = 1;
      OmniChassis.base->Go2PointStatus.enable = 1;
      OmniChassis.base->Go2PointStatus.start = 1;
      BaseChassis.Go2PointStatus.start = 1;
      last_point++;
      OmniChassis.base->Go2PointStatus.target_point.x = point_groupx[last_point];
      OmniChassis.base->Go2PointStatus.target_point.y = point_groupy[last_point];
      OmniChassis.base->ctrl_mode = CTRL_MODE_WAIT;
      IR_start_flag = 1;
      IR_cnt_ms = 0;
      if (last_point == 1)
      {
        for (int j = 1; j < 19; j++)
        {
          if (openmv1[j] > openmv2[j] && openmv1[j] > openmv3[j])
            opencv[j] = 1;
          else if (openmv2[j] > openmv1[j] && openmv2[j] > openmv3[j])
            opencv[j] = 2;
          else if (openmv3[j] > openmv1[j] && openmv3[j] > openmv2[j])
            opencv[j] = 3;
        }
        for (int i = 1; i < 7; i++)
        {
          sprintf(s1, "%d", opencv[i]);
          s1++;
        }
        for (int i = 7; i < 13; i++)
        {
          sprintf(s2, "%d", opencv[i]);
          s2++;
        }
        for (int i = 13; i < 19; i++)
        {
          sprintf(s3, "%d", opencv[i]);
          s3++;
        }
        for (int i = 0; i < 6; i++)
          HMI_SWITCH_AUDIO_18[i + 8] = str1[i];
        for (int i = 0; i < 6; i++)
          HMI_SWITCH_AUDIO_20[i + 8] = str2[i];
        for (int i = 0; i < 6; i++)
          HMI_SWITCH_AUDIO_19[i + 8] = str3[i];
      }
    }
    if (OmniChassis.base->ctrl_mode == CTRL_MODE_WAIT)
    {
      if (inwaitexeflag == 1)
      {

        test_ms = 0;
        inwaitexeflag = 0;
        if (opencv[last_point] == 1)
        {
          PLAY_AUDIO_0(&huart3);
          if (last_point == 1)
          {
            PLAY_AUDIO_18(&huart3);
            PLAY_AUDIO_19(&huart3);
            PLAY_AUDIO_20(&huart3);
            PLAY_AUDIO_21(&huart3);
          }
          //PLAY_AUDIO_3(&huart3);
        }
        else if (opencv[last_point] == 2)
        {
          PLAY_AUDIO_1(&huart3);
          if (last_point == 1)
          {
            PLAY_AUDIO_18(&huart3);
            PLAY_AUDIO_19(&huart3);
            PLAY_AUDIO_20(&huart3);
            PLAY_AUDIO_21(&huart3);
          }
          //PLAY_AUDIO_4(&huart3);
        }
        else if (opencv[last_point] == 3)
        {
          PLAY_AUDIO_2(&huart3);
          if (last_point == 1)
          {
            PLAY_AUDIO_18(&huart3);
            PLAY_AUDIO_19(&huart3);
            PLAY_AUDIO_20(&huart3);
            PLAY_AUDIO_21(&huart3);
          }
          //PLAY_AUDIO_5(&huart3);
        }
      }
      // if (penshuicishu[last_point] < openmv[last_point])
      if (penshuicishu[last_point] < opencv[last_point])
      {
        if (test_ms == 600)
        {
          HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
          HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
        }
        if (test_ms == 1000)
        {
          HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
          HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
          test_ms = 0;
          penshuicishu[last_point]++;
        }
      }
    }
    //if(OmniChassis.base->Go2PointStatus.target_point.x-OmniChassis.base->PostureStatus.x<20&&OmniChassis.base->Go2PointStatus.target_point.x-OmniChassis.base->PostureStatus.x>-20)
    //{
    if (IR_cnt_ms > 4500)
    {
      IR_start_flag = 0;
    }
    //}
    //else IR_start_flag = 0;
    break;
  case BIR:
    BaseChassis.Go2PointStatus.max_speed = 1;
    //if(last_B_flag!=B_flag) test_ms=0;
    //last_B_flag=B_flag;
    if (OmniChassis.base->PostureStatus.y > 800 && B_start_flag == 1 && OmniChassis.base->PostureStatus.x <= 3700)
    {
      B_flag++;
      B_start_flag = 0;
      test_ms = 0;
    }
    if (OmniChassis.base->PostureStatus.x <= 3700 && OmniChassis.base->PostureStatus.x > 100 && B_flag != 0)
    {
      // HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
      // HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
      if (OmniChassis.base->PostureStatus.x <= 3700 && OmniChassis.base->PostureStatus.x > 3100)
      {
        if (penshuicishu[B_flag + 11] < opencv[B_flag + 11])
        {
          //uprintf("%d",test_ms);
          if (test_ms == 200)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
          }
          if (test_ms == 400)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
            test_ms = 0;
            penshuicishu[B_flag + 11]++;
          }
        }
      }
      else if (OmniChassis.base->PostureStatus.x <= 3100 && OmniChassis.base->PostureStatus.x > 2500)
      {

        if (penshuicishu[B_flag + 11] < opencv[B_flag + 11])
        {
          if (test_ms == 200)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
          }
          if (test_ms == 400)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
            test_ms = 0;
            penshuicishu[B_flag + 11]++;
          }
        }
      }
      else if (OmniChassis.base->PostureStatus.x <= 2500 && OmniChassis.base->PostureStatus.x > 1900)
      {

        if (penshuicishu[B_flag + 11] < opencv[B_flag + 11])
        {
          if (test_ms == 200)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
          }
          if (test_ms == 400)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
            test_ms = 0;
            penshuicishu[B_flag + 11]++;
          }
        }
      }
      else if (OmniChassis.base->PostureStatus.x <= 1900 && OmniChassis.base->PostureStatus.x > 1300)
      {

        if (penshuicishu[B_flag + 11] < opencv[B_flag + 11])
        {
          if (test_ms == 200)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
          }
          if (test_ms == 400)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
            test_ms = 0;
            penshuicishu[B_flag + 11]++;
          }
        }
      }
      else if (OmniChassis.base->PostureStatus.x <= 1300 && OmniChassis.base->PostureStatus.x > 700)
      {

        if (penshuicishu[B_flag + 11] < opencv[B_flag + 11])
        {
          if (test_ms == 200)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
          }
          if (test_ms == 400)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
            test_ms = 0;
            penshuicishu[B_flag + 11]++;
          }
        }
      }
      else if (OmniChassis.base->PostureStatus.x <= 700 && OmniChassis.base->PostureStatus.x > 100)
      {

        if (penshuicishu[B_flag + 11] < opencv[B_flag + 11])
        {
          if (test_ms == 200)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_SET);
          }
          if (test_ms == 400)
          {
            HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
            test_ms = 0;
            penshuicishu[B_flag + 11]++;
          }
        }
      }
    }
    else
    {
      HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
      HAL_GPIO_WritePin(gas_push2_GPIO_Port, gas_push2_Pin, GPIO_PIN_RESET);
    }

    if (OmniChassis.base->PostureStatus.x <= 3700 && OmniChassis.base->PostureStatus.x > 3100 && B_flag == 1)
    {
      if (opencv[B_flag + 12] == 1)
        PLAY_AUDIO_0(&huart3);
      //PLAY_AUDIO_12(&huart3);
      else if (opencv[B_flag + 12] == 2)
        PLAY_AUDIO_1(&huart3);
      //PLAY_AUDIO_13(&huart3);
      else if (opencv[B_flag + 12] == 3)
        PLAY_AUDIO_2(&huart3);
      //PLAY_AUDIO_14(&huart3);
      B_flag++;
    }
    else if (OmniChassis.base->PostureStatus.x <= 3100 && OmniChassis.base->PostureStatus.x > 2500 && B_flag == 2)
    {
      if (opencv[B_flag + 12] == 1)
        PLAY_AUDIO_0(&huart3);
      //PLAY_AUDIO_12(&huart3);
      else if (opencv[B_flag + 12] == 2)
        PLAY_AUDIO_1(&huart3);
      //PLAY_AUDIO_13(&huart3);
      else if (opencv[B_flag + 12] == 3)
        PLAY_AUDIO_2(&huart3);
      //PLAY_AUDIO_14(&huart3);
      //B_flag++;
      B_start_flag = 1;
    }
    else if (OmniChassis.base->PostureStatus.x <= 2500 && OmniChassis.base->PostureStatus.x > 1900 && B_flag == 3)
    {
      if (opencv[B_flag + 12] == 1)
        PLAY_AUDIO_0(&huart3);
      //PLAY_AUDIO_12(&huart3);
      else if (opencv[B_flag + 12] == 2)
        PLAY_AUDIO_1(&huart3);
      //PLAY_AUDIO_13(&huart3);
      else if (opencv[B_flag + 12] == 3)
        PLAY_AUDIO_2(&huart3);
      //PLAY_AUDIO_14(&huart3);
      //B_flag++;
      B_start_flag = 1;
    }
    else if (OmniChassis.base->PostureStatus.x <= 1900 && OmniChassis.base->PostureStatus.x > 1300 && B_flag == 4)
    {
      if (opencv[B_flag + 12] == 1)
        PLAY_AUDIO_0(&huart3);
      //PLAY_AUDIO_12(&huart3);
      else if (opencv[B_flag + 12] == 2)
        PLAY_AUDIO_1(&huart3);
      //PLAY_AUDIO_13(&huart3);
      else if (opencv[B_flag + 12] == 3)
        PLAY_AUDIO_2(&huart3);
      //PLAY_AUDIO_14(&huart3);
      //B_flag++;
      B_start_flag = 1;
    }
    else if (OmniChassis.base->PostureStatus.x <= 1300 && OmniChassis.base->PostureStatus.x > 700 && B_flag == 5)
    {
      if (opencv[B_flag + 12] == 1)
        PLAY_AUDIO_0(&huart3);
      //PLAY_AUDIO_12(&huart3);
      else if (opencv[B_flag + 12] == 2)
        PLAY_AUDIO_1(&huart3);
      //PLAY_AUDIO_13(&huart3);
      else if (opencv[B_flag + 12] == 3)
        PLAY_AUDIO_2(&huart3);
      //PLAY_AUDIO_14(&huart3);
      //B_flag++;
      B_start_flag = 1;
    }
    else if (OmniChassis.base->PostureStatus.x <= 700 && OmniChassis.base->PostureStatus.x > 100 && B_flag == 6)
    {
      if (opencv[B_flag + 12] == 1)
        PLAY_AUDIO_0(&huart3);
      //PLAY_AUDIO_12(&huart3);
      else if (opencv[B_flag + 12] == 2)
        PLAY_AUDIO_1(&huart3);
      //PLAY_AUDIO_13(&huart3);
      else if (opencv[B_flag + 12] == 3)
        PLAY_AUDIO_2(&huart3);
      //PLAY_AUDIO_14(&huart3);
      //B_flag=0;
      B_start_flag = 1;
    }
    if (IR_cnt_ms > 500)
    {
      if (fabs(OmniChassis.base->PostureStatus.x - lastposture_x) < 10 && fabs(OmniChassis.base->PostureStatus.y - lastposture_y) < 10)
      {
        OmniChassis.base->ctrl_mode = CTRL_MODE_HANDLE;
        kasi_flag = 0;
      }
      if (OmniChassis.base->ctrl_mode == CTRL_MODE_HANDLE && kasi_flag == 3)
      {
        OmniChassis.base->ctrl_mode = CTRL_MODE_TRACK;
        //BaseChassis.TrackStatus.point_index=BaseChassis.TrackStatus.point_index+2;
      }
      kasi_flag++;
      lastposture_x = OmniChassis.base->PostureStatus.x;
      lastposture_y = OmniChassis.base->PostureStatus.y;
      IR_cnt_ms = 0;
    }
    //if (OmniChassis.base->Go2PointStatus.enable == 0 && last_point != 12)
    //  {
    //       OmniChassis.base->Go2PointStatus.enable = 1;
    //       OmniChassis.base->Go2PointStatus.start = 1;
    //       BaseChassis.Go2PointStatus.start = 1;
    //       last_point++;
    //        OmniChassis.base->Go2PointStatus.target_point.x = point_groupx[last_point];
    //       OmniChassis.base->Go2PointStatus.target_point.y = point_groupy[last_point];
    //       IR_cnt_ms = 0;
    //  }
    break;
  case CIR:
    BaseChassis.Go2PointStatus.max_speed = 0.8;
    if (OmniChassis.base->Go2PointStatus.enable == 0 && last_point != 19)
    {
      //inwaitexeflag = 1;
      OmniChassis.base->Go2PointStatus.enable = 1;
      OmniChassis.base->Go2PointStatus.start = 1;
      BaseChassis.Go2PointStatus.start = 1;
      last_point++;
      OmniChassis.base->Go2PointStatus.target_point.x = point_groupx[last_point];
      OmniChassis.base->Go2PointStatus.target_point.y = point_groupy[last_point];
      if (last_point == 12)
      {
        OmniChassis.base->Go2PointStatus.target_yaw = -1.57;
        OmniChassis.base->ctrl_mode = CTRL_MODE_TUNING;
        uprintf("CTRL_MODE_TUNING!");
      }
      if (last_point == 16)
      {
        uprintf("IR_cnt_ms=%d", IR_cnt_ms);
        IR_status = DIR;
        inwaitexeflag = 0;
        IR_start_flag = 1;
        c_last_ms = 0;
        uprintf("stateD!");
      }
      //OmniChassis.base->ctrl_mode = CTRL_MODE_WAIT;
      //IR_start_flag = 1;
      //IR_cnt_ms = 0;
    }

    //if(HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin)==1&&inwaitexeflag == 0)
    if (HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin) == 1 && last_laser == 0 && last_point != 12 && IR_cnt_ms > 1000)
    {
      OmniChassis.base->ctrl_mode = CTRL_MODE_WAIT;
      inwaitexeflag = 1;
      IR_cnt_ms = 0;
      C_flag = 1;
    }
    if (c_last_ms > 10)
    {
      last_laser = HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin);
      c_last_ms = 0;
    }
    if (OmniChassis.base->ctrl_mode == CTRL_MODE_WAIT)
    {
      if (C_flag == 1)
      {
        test_ms = 0;
        C_flag = 0;
        if (opencv[cquxishu] == 1)
          PLAY_AUDIO_0(&huart3);
        //PLAY_AUDIO_15(&huart3);
        else if (opencv[cquxishu] == 2)
          PLAY_AUDIO_1(&huart3);
        //PLAY_AUDIO_16(&huart3);
        else if (opencv[cquxishu] == 3)
          PLAY_AUDIO_2(&huart3);
        // PLAY_AUDIO_17(&huart3);
      }
      if (penshuicishu[cquxishu] < opencv[cquxishu])
      {
        if (test_ms == 600)
        {
          HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
        }
        if (test_ms == 1000)
        {
          HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
          test_ms = 0;
          penshuicishu[cquxishu]++;
        }
      }
      if (IR_cnt_ms > 4500)
      {
        if (cquxishu == 7 && last_point <= 11)
        {
         /* if (HAL_GPIO_ReadPin(gas_pull3_GPIO_Port, gas_pull3_Pin) == 1)
          {*/
          if (HAL_GPIO_ReadPin(kaiguan_GPIO_Port, kaiguan_Pin) == 0)
          {
            for (int i = 12; i < 23; i++)
            {
              point_groupy[i] = point_groupy[i] + 45;
            }
          }
          else if (HAL_GPIO_ReadPin(kaiguan_GPIO_Port, kaiguan_Pin) == 1)
          {
            for (int i = 12; i < 23; i++)
            {
              point_groupy[i] = point_groupy[i] + 25;
            }
          }
        }
            /*for (int i = 12; i < 20; i++)
            {
              point_groupy[i] = point_groupy[i] + 25;
            }
            for (int i = 8; i < 12; i++)
            {
              point_groupy[i] = point_groupy[i] + 20;
            }
          }
          else if (HAL_GPIO_ReadPin(gas_pull3_GPIO_Port, gas_pull3_Pin) != 1 && HAL_GPIO_ReadPin(gas_pull2_GPIO_Port, gas_pull2_Pin) == 1)
          {
            for (int i = 12; i < 23; i++)
            {
              point_groupy[i] = point_groupy[i] + 80;
            }
          }
        }*/
        OmniChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
        IR_cnt_ms = 0;
        if (last_point <= 11)
          cquxishu++;
        else
          cquxishu--;
      }
    }
    if (HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin) == 0)
      inwaitexeflag = 0;
    if (last_point == 15 && OmniChassis.base->PostureStatus.x < -350)
    {
      if (OmniChassis.base->ctrl_mode == CTRL_MODE_GO_TO_POINT)
      {
        IR_cnt_ms = 0;
        OmniChassis.base->ctrl_mode = CTRL_MODE_HANDLE;
      }
      else if (OmniChassis.base->ctrl_mode == CTRL_MODE_HANDLE && IR_cnt_ms > 1500)
      {
        OmniChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
        last_point = 16;
        OmniChassis.base->Go2PointStatus.target_point.x = point_groupx[last_point];
        OmniChassis.base->Go2PointStatus.target_point.y = point_groupy[last_point];
        IR_status = DIR;
      }

      uprintf("gg");
    }
    // HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
    //else
    // HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
    break;
  case DIR:
    BaseChassis.Go2PointStatus.max_speed = 0.8;
    if (OmniChassis.base->Go2PointStatus.enable == 0 && last_point != 22)
    {
      //inwaitexeflag = 1;
      OmniChassis.base->Go2PointStatus.enable = 1;
      OmniChassis.base->Go2PointStatus.start = 1;
      BaseChassis.Go2PointStatus.start = 1;
      last_point++;
      OmniChassis.base->Go2PointStatus.target_point.x = point_groupx[last_point];
      OmniChassis.base->Go2PointStatus.target_point.y = point_groupy[last_point];
      if (last_point == 20)
      {
        OmniChassis.base->Go2PointStatus.target_yaw = 1.57;
        OmniChassis.base->ctrl_mode = CTRL_MODE_TUNING;
        uprintf("CTRL_MODE_TUNING!");
      }
      //OmniChassis.base->ctrl_mode = CTRL_MODE_WAIT;
      //IR_start_flag = 1;
      //IR_cnt_ms = 0;
    }

    //if(HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin)==1&&inwaitexeflag == 0)
    if (HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin) == 1 && last_laser == 0 && last_point != 20 && IR_cnt_ms > 1000)
    {
      OmniChassis.base->ctrl_mode = CTRL_MODE_WAIT;
      inwaitexeflag = 1;
      IR_cnt_ms = 0;
      D_flag = 1;
    }
    if (c_last_ms > 10)
    {
      last_laser = HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin);
      c_last_ms = 0;
    }
    if (OmniChassis.base->ctrl_mode == CTRL_MODE_WAIT)
    {
      detect_num++;
      totalD = totalD + D_boardarea;
      //uprintf("D_boardarea=%d\r\n",D_boardarea);
      //if (D_flag == 1&&test_ms == 1100)
      if (D_flag == 1 && IR_cnt_ms >= 1200)
      {
        test_ms = 0;
        D_flag = 0;
        totalD = totalD / detect_num;
        uprintf("totalD=%d\r\n", totalD);
        uprintf("D_boardarea=%d\r\n", D_boardarea);
       if (HAL_GPIO_ReadPin(gas_pull2_GPIO_Port, gas_pull2_Pin) == 1)
        {
          if (HAL_GPIO_ReadPin(gas_pull3_GPIO_Port, gas_pull3_Pin) == 1)
          {
            distance = 1;
          }
          else
            distance = 2;
        }
        else
          distance = 3;
        if (distance == 1)
        {
          if (D_boardarea <= 80)
          {
            PLAY_AUDIO_6(&huart3);
            PLAY_AUDIO_9(&huart3);
            size = 1;
          }
          else if (D_boardarea <= 125 && D_boardarea >= 86)
          {
            PLAY_AUDIO_7(&huart3);
            PLAY_AUDIO_10(&huart3);
            size = 2;
          }
          else if (D_boardarea >= 141)
          {
            PLAY_AUDIO_8(&huart3);
            PLAY_AUDIO_11(&huart3);
            size = 3;
          }
          else if (D_boardarea < 141 && D_boardarea > 125)
          {
            if (fabs(D_boardarea - 125) < fabs(D_boardarea - 141))
            {
              PLAY_AUDIO_7(&huart3);
              PLAY_AUDIO_10(&huart3);
              size = 2;
            }
            else
            {
              PLAY_AUDIO_8(&huart3);
              PLAY_AUDIO_11(&huart3);
              size = 3;
            }
          }
          else if (D_boardarea > 80 && D_boardarea < 86)
          {
            if (fabs(D_boardarea - 86) < fabs(D_boardarea - 80))
            {
              PLAY_AUDIO_7(&huart3);
              PLAY_AUDIO_10(&huart3);
              size = 2;
            }
            else
            {
              PLAY_AUDIO_6(&huart3);
              PLAY_AUDIO_9(&huart3);
              size = 1;
            }
          }
        }
        else if (distance == 2)
        {
          if (D_boardarea <= 60)
          {
            PLAY_AUDIO_6(&huart3);
            PLAY_AUDIO_9(&huart3);
            size = 1;
          }
          else if (D_boardarea <= 86 && D_boardarea >= 55)
          {
            PLAY_AUDIO_7(&huart3);
            PLAY_AUDIO_10(&huart3);
            size = 2;
          }
          else if (D_boardarea >= 101)
          {
            PLAY_AUDIO_8(&huart3);
            PLAY_AUDIO_11(&huart3);
            size = 3;
          }
          else if (D_boardarea < 101 && D_boardarea > 86)
          {
            if (fabs(D_boardarea - 86) < fabs(D_boardarea - 101))
            {
              PLAY_AUDIO_7(&huart3);
              PLAY_AUDIO_10(&huart3);
              size = 2;
            }
            else
            {
              PLAY_AUDIO_8(&huart3);
              PLAY_AUDIO_11(&huart3);
              size = 3;
            }
          }
          else if (D_boardarea < 55 && D_boardarea > 60)
          {
            if (fabs(D_boardarea - 55) < fabs(D_boardarea - 60))
            {
              PLAY_AUDIO_7(&huart3);
              PLAY_AUDIO_10(&huart3);
              size = 2;
            }
            else
            {
              PLAY_AUDIO_6(&huart3);
              PLAY_AUDIO_9(&huart3);
              size = 1;
            }
          }
        }
        else if (distance == 3)
        {
          if (D_boardarea <= 46)
          {
            PLAY_AUDIO_6(&huart3);
            PLAY_AUDIO_9(&huart3);
            size = 1;
          }
          else if (D_boardarea <= 55 && D_boardarea >= 41)
          {
            PLAY_AUDIO_7(&huart3);
            PLAY_AUDIO_10(&huart3);
            size = 2;
          }
          else if (D_boardarea >= 71)
          {
            PLAY_AUDIO_8(&huart3);
            PLAY_AUDIO_11(&huart3);
            size = 3;
          }
          else if (D_boardarea < 71 && D_boardarea > 55)
          {
            if (fabs(D_boardarea - 55) < fabs(D_boardarea - 71))
            {
              PLAY_AUDIO_7(&huart3);
              PLAY_AUDIO_10(&huart3);
              size = 2;
            }
            else
            {
              PLAY_AUDIO_8(&huart3);
              PLAY_AUDIO_11(&huart3);
              size = 3;
            }
          }
          else if (D_boardarea < 41 && D_boardarea > 46)
          {
            if (fabs(D_boardarea - 41) < fabs(D_boardarea - 46))
            {
              PLAY_AUDIO_7(&huart3);
              PLAY_AUDIO_10(&huart3);
              size = 2;
            }
            else
            {
              PLAY_AUDIO_6(&huart3);
              PLAY_AUDIO_9(&huart3);
              size = 1;
            }
          }
        }
        //totalD=0;
        //}
        //else if(distance==2)
        //{

        //}
        //else if(distance==3)
        //{

        //}
        //if(opencv[cquxishu]==1)
        //PLAY_AUDIO_0(&huart3);
        //PLAY_AUDIO_15(&huart3);
        //else if(opencv[cquxishu]==2)
        //{PLAY_AUDIO_1(&huart3);
        //PLAY_AUDIO_16(&huart3);}
        //else if(opencv[cquxishu]==3)
        //{PLAY_AUDIO_2(&huart3);
        //PLAY_AUDIO_17(&huart3);}
      }
      if (penshuicishu[dquxishu] < size)
      {
        if (test_ms == 600)
        {
          HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_SET);
        }
        if (test_ms == 1000)
        {
          HAL_GPIO_WritePin(gas_push_GPIO_Port, gas_push_Pin, GPIO_PIN_RESET);
          test_ms = 0;
          penshuicishu[dquxishu]++;
        }
      }
      if (IR_cnt_ms > 4500)
      {
        OmniChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
        detect_num = 0;
        totalD = 0;
        IR_cnt_ms = 0;
        if (last_point <= 19)
          dquxishu++;
        else
          dquxishu--;
      }
    }
    if (HAL_GPIO_ReadPin(gas_pull_GPIO_Port, gas_pull_Pin) == 0)
      inwaitexeflag = 0;

    if (OmniChassis.base->ctrl_mode == CTRL_MODE_HANDLE && IR_cnt_ms > 1500)
    {
      OmniChassis.base->ctrl_mode = CTRL_MODE_GO_TO_POINT;
      last_point = 16;
      OmniChassis.base->Go2PointStatus.target_point.x = point_groupx[last_point];
      OmniChassis.base->Go2PointStatus.target_point.y = point_groupy[last_point];
      uprintf("gg");
    }

    break;
  default:
    break;
  }
}

#endif